//
// Created by chen on 25-5-10.
//

#include "SVPWM.h"
#include <cmath>
#include <algorithm>  // 包含 std::clamp
// 极对数（根据实际电机设置，示例为7）
#define POLE_PAIRS 7

SVPWM::SVPWM(float dcBusVoltage) : dcBusVoltage(dcBusVoltage) {}

// SVPWM.cpp
#include "SVPWM.h"

// 改进的测试代码：固定电压矢量旋转
void SVPWM::modulate(float valpha, float vbeta, float mechanicalAngle, float& dutyU, float& dutyV, float& dutyW) {
    // ---------------------- 1. 机械角度转电角度并归一化 ----------------------
    float electricalAngle = mechanicalAngle * POLE_PAIRS; // 计算电角度（机械角度 × 极对数）
    float angle = fmodf(electricalAngle, 2.0f * M_PI);     // 归一化到 [0, 2π)
    if (angle < 0.0f) angle += 2.0f * M_PI;

    // ---------------------- 2. 计算扇区（1-6）----------------------
    int sector = static_cast<int>(angle * 3.0f / M_PI) + 1; // 每 π/3 弧度为一个扇区
    sector = (sector % 6 == 0) ? 6 : (sector % 6);         // 确保扇区值在 1-6 范围内

    // ---------------------- 3. 电压矢量归一化（防止过调制）----------------------
    float vref = std::sqrt(valpha * valpha + vbeta * vbeta);
    const float maxModulation = 0.907f; // 最大调制比（对应六边形内切圆）
    if (vref > maxModulation) {
        float scale = maxModulation / vref;
        valpha *= scale;
        vbeta *= scale;
    }

    // ---------------------- 4. 计算基本矢量作用时间（X/Y/Z 分量法）----------------------
    float X = vbeta;
    float Y = (std::sqrt(3.0f) * valpha + vbeta) / 2.0f;
    float Z = (-std::sqrt(3.0f) * valpha + vbeta) / 2.0f;

    float T1 = 0, T2 = 0; // 相邻基本矢量作用时间
    switch (sector) {
        case 1: T1 = Z;  T2 = Y;  break; // 扇区1：V0, V1, V2
        case 2: T1 = X;  T2 = Z;  break; // 扇区2：V1, V2, V3
        case 3: T1 = -Y; T2 = X;  break; // 扇区3：V2, V3, V4
        case 4: T1 = -Z; T2 = -Y; break; // 扇区4：V3, V4, V5
        case 5: T1 = -X; T2 = -Z; break; // 扇区5：V4, V5, V0
        case 6: T1 = Y;  T2 = -X; break; // 扇区6：V5, V0, V1
    }

    // 时间归一化（确保作用时间在 [0, 1] 范围内）
    T1 = std::max(0.0f, std::min(T1, 1.0f));
    T2 = std::max(0.0f, std::min(T2, 1.0f));
    float T0 = 1.0f - T1 - T2; // 零矢量作用时间

    // ---------------------- 5. 计算三相占空比（七段式中心对齐PWM）----------------------
    float Ta, Tb, Tc;
    switch (sector) {
        case 1:
            Ta = T0/2 + T1 + T2; // A相
            Tb = T0/2 + T2;      // B相
            Tc = T0/2;           // C相
            break;
        case 2:
            Ta = T0/2 + T1;
            Tb = T0/2 + T1 + T2;
            Tc = T0/2 + T2;
            break;
        case 3:
            Ta = T0/2;
            Tb = T0/2 + T1 + T2;
            Tc = T0/2 + T1;
            break;
        case 4:
            Ta = T0/2 + T2;
            Tb = T0/2;
            Tc = T0/2 + T1 + T2;
            break;
        case 5:
            Ta = T0/2 + T1;
            Tb = T0/2 + T2;
            Tc = T0/2 + T1 + T2;
            break;
        case 6:
            Ta = T0/2 + T1 + T2;
            Tb = T0/2 + T1;
            Tc = T0/2 + T2;
            break;
        default:
            Ta = Tb = Tc = 0.5f; // 安全默认值（零矢量）
    }

    // ---------------------- 6. 占空比限制（防止硬件直通）----------------------
    const float minDuty = 0.01f; // 最小占空比（需根据死区时间调整）
    const float maxDuty = 0.99f; // 最大占空比
    dutyU = std::clamp(Ta, minDuty, maxDuty);
    dutyV = std::clamp(Tb, minDuty, maxDuty);
    dutyW = std::clamp(Tc, minDuty, maxDuty);
}

// 计算扇区（1~6），改进版
int SVPWM::calculateSector(float angle) {
    // 确保角度在 [0, 2π) 范围内
    angle = fmodf(angle, 2.0f * M_PI);
    if (angle < 0) angle += 2.0f * M_PI;

    // 更精确的扇区计算方法
    // 将 [0, 2π) 分为6个扇区，每个扇区π/3弧度
    int sector = static_cast<int>(angle * 3.0f / M_PI) + 1;

    // 处理边界情况，确保扇区在1~6之间
    if (sector == 7) sector = 1;  // 当angle接近2π时，可能计算出7，修正为1

    return sector;
}

